% !TEX root = verslag_main.tex
\section{Building the LQR Controller}
\label{sec:controller}
\subsection{Designing the LQR Controller}
\label{sec:designinglqr}
Since the states $x$ and $\theta$ are measured and the states $\dot{x}$ and $\dot{\theta}$ can easily be calculated. Meaning that it will not be necessary to design a state estimator. The states can be directly fed into the LQR controller which in it's turn produces an appropriate input for the system to obtain the desired output. The controller is designed in continuous time but is implemented in a digital computer i.e. discrete time. If the sampling time of the computer is small enough we can think of this as (an approximation of) continuous time. The controller samples the voltages with sampling frequency $f_{s} = 200$ Hz (sampling time $T_{s} = 0.005$ s). This is fast enough compared to the dynamics of the system but not too fast for the computer to handle. One obtains only information about frequencies in the signal lower than half the sampling frequency.
 The LQR-controller determines an optimal state-feedback gain K, such that the closed-loop system $A-BK$ minimizes the quadratic performance index given by the following equation
\[
J = \int(x^{t}Qx + u^{t}Ru)dt
\]
This can be achieved by using the command \emph{lqr} in Matlab. To level the seesaw at a certain angle $\theta_{desired}$ we calculate the state-feedback not with $u=-Kx$, but with $u=-K(x-x_{d})$ where $x_{d}$ equals $[0 \hspace{0.2cm} \theta_{desired} \hspace{0.2cm} 0 \hspace{0.2cm} 0]$. In this implementation it is thus assumed that $x_{desired} = 0$. This is obviously not a stable situation since the cart will have to move to maintain the desired angle. This means that $x_{desired}$ is never an equilibrium point except when $\theta_{desired} = 0$. 

%%%%%%%%%%%%%%%%%%%%%%%%%%%

\subsection{Closed-loop analysis}
The closed-loop system needs to be stable. If we check the eigenvalues of the closed-loop system $A-BK$ we find that these are all in the left half plane and thus the system is stable. More specifically for the weighting matrices $Q = diag([1000, 4000, 0, 0])$ and $R = 10$ we find the following poles: $  -16.6331,  -2.5056 \pm 2.5630i, -4.1851$. These are shown in figure \ref{fig:polesClosedLoop}. This is a second order system with dominant poles $-2.5056 \pm 2.5630i$. This system has a damping ratio of $\varsigma = 0.699$ and natural frequency $\omega_{n} = 3.58427$. The results are shown in table \ref{table:timeValues}. The reaction of the system to a step input is shown in figure \ref{fig:stepResponse}. 


\setlength\figureheight{4cm} \setlength\figurewidth{4cm}
\begin{figure}[h]
  \centering
  \input{img/stepResponse.tikz}
  \caption{The step response of the closed-loop system. Left: the position $x$, right: the angle $\theta$}
  \label{fig:stepResponse}
\end{figure}

\begin{table}[htp]
\centering
	\begin{tabular}{| l | l |}
	\hline
	damping ratio $\varsigma$ & 0.699 \\ \hline
	natural frequency
	rise time $t_{r} \simeq 1.8/\omega_{n}$ & 0.50219 \\ \hline
	settling time $t_{s} = 4.6/\varsigma\omega_{n}$ & 1.8359 \\ \hline
	peak time $t_{p} = \pi/\omega_{n}\sqrt{1-\varsigma^{2}}$ &  1.22575\\ \hline
	overshoot $M_{p} = e^{-\pi\varsigma/\sqrt{1-\varsigma^{2}}}$ &  0.046364\\ \hline 
	\end{tabular}
\vspace{0.1cm}
\caption{The time response values for the dominant poles $-2.5056 \pm 2.5630i$ of the closed-loop system}
\label{table:timeValues}
\end{table}



%%%%%%%%%%%%%%%%%%%%%%%%%

\subsection{Testing the LQR controller}
\label{sec:testinglqr}
The state-feedback gain K is tested on the linearised model in two different Simulink schemes, a simple one and a more advanced one. The schemes can be found in appendix \ref{sec:simulinkschemes}, or in the accompanying model files.

The simple Simulink scheme consists of the continuous time model and the controller. The second, more advanced one tries to mimic the behaviour of the real setup as close as possible. Several aspects of the setup have to be taken into account. All of these modifications will affect the behaviour of the controller
\begin{itemize}
\item	The velocities $\dot{x}$ and $\dot{\theta}$ cannot be measured directly, making the position $x$ and the angle $\theta$ the only outputs. Therefore the C matrix of the state space model is  
$$ C=\begin{bmatrix}
1 & 0 & 0 & 0  \\ 
0 & 1 & 0 & 0 \\ \end{bmatrix}$$

\item In reality the measured voltages will be disturbed by noise. The noise energy is chosen to be an estimate of the noise on the real setup.

\item The measured voltages indicating the angle and the position of the cart are discrete signals, whereas the output of the model is in continuous time. These values thus have to be discretized and quantized. In Simulink this is established by passing the output from the model through a zero-order hold and a quantizer. The zero-order hold samples the input signal at a sampling frequency $f_{s} = 200$ Hz. The quantizer takes the finite word length into account.

\item The velocities $\dot{x}$ and $\dot{\theta}$ have to be derived from the outputs of the system. They are calculated using the backward difference equation. For this the output needs to be filtered with a low-pass filter with a cut-off frequency of around $2$ Hz. This is needed because the calculation of these derivatives is very sensitive to fluctuations and noise. Since this does not only filter away the noise but also the high frequency components of the signal this filter should only be used to calculate $\dot{x}$ and $\dot{\theta}$. This means that the measured states $x$ and $\theta$ are to be given to the controller without passing through this low-pass filter. The effects of this filter on the performance of the controller are studied in section \ref{sec:proofofpudding}.

\item The input voltage is bounded by the specifications of the motor on the real setup. In Simulink a saturation block is added to limit the voltage to $\pm 5$ volts (even though the electrical motor accepts voltages between $\pm 6$ volts). In practice there is a D/A-converter to transform the input voltage to an analogue signal. In this case this is not necessary because Simulink can interpret the digital signal as a continuous signal. The saturation on the input will make the controller slower, because the desired amount of action cannot always be applied.
\end{itemize}

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

\subsection{Influence of Q and R}
As described in section \ref{sec:designinglqr}, the gain matrix will minimize the quadratic performance index given by
\[
J = \int(x^{t}Qx + u^{t}Ru)dt
\]
The matrixes Q and R each influence the resulting control law K and thus the behavior of the system. Increasing or decreasing an element of Q (we take Q to be a positive semi-definite diagonal matrix) will make the contribution of the corresponding state to the performance index larger or smaller respectively. For example, we are only interested in making the position and the angle of the cart zero, so we decrease the elements of Q corresponding to the velocities to zero. Increasing or decreasing of R will make limiting the value of the input more important or less important respectively, with R=0 corresponding to an unlimited input. In our case the input is bounded by the motors, so it is best to choose R high enough in order to minimize the effect of the saturation.  

We first analyse the two Simulink schemes with the given weighting matrices as starting points.
\[
Q = \begin{bmatrix}
1000 & 0 & 0 & 0 \\
0 & 4000 & 0 & 0 \\
0 & 0 & 0 & 0 \\
0 & 0 & 0 & 0
\end{bmatrix}, \hspace{2cm} R = 10
\]

 Figure \ref{fig:controllerSim} shows the position of the cart and the angle of the seesaw when the seesaw is initialized at an angle of $-0.05$ rad. Figure \ref{fig:simpleSimulink} shows the results of the simulation made by the simple Simulink scheme and \ref{fig:advancedSimulink} shows the simulation results of the more advanced Simulink scheme. From these figures it is obvious that the angle of the seesaw first increases a bit and then decreases again because of the actions undertaken by the controller. This is as one would expect to happen. The simple Simulink is a bit more optimistic than the more advanced one. We see that the controller is able to stabilize the seesaw at an angle $\theta = 0$ within the second. The advanced scheme needs about 2 seconds and does this in a more oscillatory way. This is due to the modifications of the scheme listed in section \ref{sec:testinglqr}.

\setlength\figureheight{3cm} \setlength\figurewidth{5cm}
\begin{figure}[h]
\centering
  \subfloat[simple Simulink scheme]{ \label{fig:simpleSimulink} \input{img/firstSimulinkControllerRespons.tikz}}
  \subfloat[more advanced Simulink scheme]{ \label{fig:advancedSimulink} \input{img/secondSimulinkControllerRespons.tikz}}
  \caption{Top: The blue curve is the position of the cart, the green curve is the angle of the seesaw. Bottom: The blue curve is the speed of the cart, the green curve is the speed of the angle. \newline The seesaw is initialised at an angle of -0.05 rad. }
  \label{fig:controllerSim}
\end{figure}

\setlength\figureheight{3cm} \setlength\figurewidth{5cm}
\begin{figure}[h]
\centering
  \subfloat[simple Simulink scheme]{ \label{fig:fastControllerSimple} \input{img/fastControllerResponsFirstSimulink.tikz}}
  \subfloat[advanced Simulink scheme]{ \label{fig:fastControllerAdvanced} \input{img/fastControllerResponsSecondSimulink.tikz}}
  \caption{Plot of the angle and the position of the cart with in initial angle of -0.05 rad. The blue curve is the position of the cart, the green curve is the angle of the seesaw.}
\end{figure}

\subsection{Tuning of Q and R}
\label{sec:Tuning}

Up until now we have worked with the weighting matrices  $Q = diag([1000, 4000, 0, 0])$ and $R = 10$ since these were provided as a starting point. We have chosen to design a fast responding controller. This means that we pay a lot of value tot the states of the system. The states are weighed by the $Q$ matrix. It is especially the first and second state that are important. Therefore it is clear that one should increase the weights of these states i.e. $Q_{11}$ and $Q_{22}$. Since we want to keep the seesaw levelled at a certain angle but we do not care too much about the position of the cart we will increase the weight of the angle $Q_{22}$. As the input voltage did not clip at this point we were able to lower the weight given to the input. This meant that higher voltages could be sent to the motor allowing for a faster response. All of this is done by means of trial-and-error. Eventually we found $Q = diag([1000, 8000, 0, 0])$ and $R = 0.2$ to be good values. The response of the closed-loop system is shown in figure \ref{fig:differentQR}. The top part of the image shows $x$ and $\theta$ for $Q = diag([1000, 4000, 0, 0])$ and $R = 10$. The bottom part of the figure shows the response with $Q = diag([1000, 8000, 0, 0])$ and $R = 0.2$}. From this figure one can see that the second controller works more swiftly as well as that the voltages send to the motor are substantially larger with the second controller, even though they remain within the imposed bounds.

\setlength\figureheight{3cm} \setlength\figurewidth{3cm}
\begin{figure}[h]

  \input{img/differentQR.tikz}
  \caption{The response of the closed-loop system to an initial angle of $\theta = -0.05$ rad. From left to right: position $x$, angle $\theta$ and voltage $V$. Top: $Q = diag([1000, 4000, 0, 0])$ and $R = 10$. Bottom: $Q = diag([1000, 8000, 0, 0])$ and $R = 0.2$}
  \label{fig:differentQR}
\end{figure}
